#include "MOTOR.h"
 
u16 PWM;

static long int Disitance_data[4] = {0,0,0,0};

//电机PID的值
static PID_Handle Motor_PID_f_data[4] = {{6.8,13.3,0,1}, 	//4轮前进P I D 缩放比例
										{8.3,13.2,0,1},	//L
										
										{14,9,0,1},
										{10,13.7,0,1}};	//L

static PID_Handle Motor_PID_b_data[4] = {{8.6,8.375,0,1}, 	//4轮后退P I D 缩放比例   L
										{9,7.9,0,1},

										{10.1,9,0,1}, 	//L
										{12.5,10.2,0,1}};

static int Motor_target_data[4] = {0,0,0,0};			//电机目标值
/*
	Motor_Init 电机初始化
*/
void Motor_Init(void)
{
	Hall_TIM_Config();
	PID_TIM_Config();
	BSP_PWM_Init();
	bsp_EXTI_Init();
}

/******************************************************
 * @name        void Distance_clear(void)
 * @date		2022 10.3
 * @function     清除霍尔计数
 * @parameters none
 * @retvalue   none
******************************************************/
void Disitance_clear(void)
{
	uint16_t n;
	for( n=0 ; n<4 ; n++)
		distance[n] = 0;
}


/************************************************************
 * @name	 	void Motor_move(u8 num,u32 speed,u8 move_flag)
 * @date		2022 11.1
 * @function	电机移动
 * @parameters	mun:电机编号
 * 				speed：电机转速(PWM差值)
 * 				move_flag:电机运动方向
 * @retvalue 	none
**************************************************************/
void Motor_move(u8 num,u32 speed,u8 move_flag)
{   if(speed<=2000)
	{
		switch (num)
		{
		case 0:PWM_Out(8,1499);//EXTI4
					if(move_flag == B_MOVE)
					{
						PWM_Out(8,0);
						PWM_Out(9,0+speed);
					}
					else if(move_flag == F_MOVE)
					{
						PWM_Out(8,2499);
						PWM_Out(9,2499-speed);
					}
			break;

		case 1:PWM_Out(7,1499);//EXTI2
					if(move_flag == B_MOVE)
					{
						PWM_Out(7,0);
						PWM_Out(6,0+speed);
					}
					else if(move_flag == F_MOVE)
					{
						PWM_Out(7,2499);
						PWM_Out(6,2499-speed);
					}
			break;

		case 2:PWM_Out(5,1499);//EXTI0
			   		if(move_flag == B_MOVE)
					{
						PWM_Out(5,0);
						PWM_Out(4,0+speed);
					}
					else if(move_flag == F_MOVE)
					{
						PWM_Out(5,2499);
						PWM_Out(4,2499-speed);
					}
					
			break;

		case 3:PWM_Out(2,1499);//EXTI8
		       		if(move_flag == B_MOVE)
					{
						PWM_Out(2,0);
						PWM_Out(3,0+speed);
					}
					else if(move_flag == F_MOVE)
					{
						PWM_Out(2,2499);
						PWM_Out(3,2499-speed);  
					}
			break;

		default:
			break;
		}	
	}

}
/************************************************************
 * @name	 	float_p Motor_PID_Out(u8 temp)
 * @date		2022 10.3
 * @function	电机PID运算值输出
 * @parameters	temp 电机编号
 * @retvalue 	Motor_PID_data[temp].Output PID运算值
**************************************************************/
float_p Motor_PID_Out(u8 temp,u8 Move_flag)
{
	if(Move_flag == F_MOVE)
		return  Motor_PID_f_data[temp].Output;
	if(Move_flag == B_MOVE)
		return  Motor_PID_b_data[temp].Output;
	else 
		return 0;
}

/************************************************************
 * @name	 	Motor_PID_Move(u8 num,int Target)
 * @date		2023 2.19
 * @function	电机PID运算值输出
 * @parameters	num 电机编号
 * 				Target 电机目标转速
 * @retvalue 	void
**************************************************************/
void Motor_PID_Move(u8 num,int Target)
{
	Motor_target(num,Target);
	if(Target>0)
	{
		Motor_target(num,Target);
    	Motor_move(num,Motor_PID_Out(num,F_MOVE),F_MOVE);
	}
	else if(Target<0)
	{
		Motor_target(num,-Target);
		Motor_move(num,Motor_PID_Out(num,B_MOVE),B_MOVE);
	}
	else if(Target == 0)
	{
		Motor_move(num,0,B_MOVE);
		Motor_PID_f_data[num].Err[0] = 0;
		Motor_PID_f_data[num].Err[1] = 0;
		Motor_PID_f_data[num].Err[2] = 0;
		Motor_PID_f_data[num].Target = 0;
		Motor_PID_f_data[num].Output = 0;

		Motor_PID_b_data[num].Err[0] = 0;
		Motor_PID_b_data[num].Err[1] = 0;
		Motor_PID_b_data[num].Err[2] = 0;
		Motor_PID_b_data[num].Target = 0;
		Motor_PID_b_data[num].Output = 0;

	}
}

/*
	Motor_target电机目标值输入
	@date 2022.10.22 吴昕灿
	@param num:		电机编号
		   Target:	电机转速目标值
*/
void Motor_target(u8 num ,u8 Target)
{

	Motor_target_data[num] = Target;
	
}
/*
	Motor_PID_Out 计算PID输出值
	@param Target :目标值
	@param read_speed:读取速度(实际速度)
*/
void Motor_PID_compute(PID_Handle*Motor_PID,int Target,long int read_speed)
{

	Motor_PID->Target = Target;
	Motor_PID->OutputMax = 2000;//输出最大值
	Motor_PID->OutputMin = 200;	//输出最小值
	PID_IncOperation(Motor_PID,read_speed);
}

/************************************************************
 * @name	 	long int* Read_Speed(num)
 * @date		2022 10.3
 * @function	读取电机速度
 * @parameters	num:读取电机编号
 * @retvalue 	none
**************************************************************/
long int Read_Speed(u8 num)
{
	return Disitance_data[num];
}


//EXTI0,2,4,8外部中断，用来霍尔计数
void EXTI9_5_IRQHandler(void)
{
	

	if(EXTI_GetITStatus(EXTI_Line8) != RESET)
	{
		//if(GPIO_ReadInputDataBit(GPIOD,GPIO_Pin_9) == 0)
		distance[3]++;
		//else 
		//distance[3]--;
		
		EXTI_ClearITPendingBit(EXTI_Line8);
		
	}
	
}
void EXTI4_IRQHandler(void)
{
	if(EXTI_GetITStatus(EXTI_Line4) != RESET)
	{
		//if(GPIO_ReadInputDataBit(GPIOC,GPIO_Pin_5) == 0)
		distance[0]++;
		//else 
		//distance[0]--;
		
		EXTI_ClearITPendingBit(EXTI_Line4);   
		                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                  
	}
}
void EXTI2_IRQHandler(void)
{
	if(EXTI_GetITStatus(EXTI_Line2) != RESET)
	{
		//if(GPIO_ReadInputDataBit(GPIOC,GPIO_Pin_3) == 0)
		distance[1]++;
		//else 
		//distance[1]--;
		
		EXTI_ClearITPendingBit(EXTI_Line2);
	
	}
}
void EXTI0_IRQHandler(void)
{
	if(EXTI_GetITStatus(EXTI_Line0) != RESET)
	{
		//if(GPIO_ReadInputDataBit(GPIOC,GPIO_Pin_1) == 0)
		distance[2]++;
		//else 
		//distance[2]--;
		
		EXTI_ClearITPendingBit(EXTI_Line0);
	
	}
}


//tim6中断 用来处理霍尔计数
void TIM6_IRQHandler(void)
{
	u8 temp = 0;
	if(TIM_GetITStatus(TIM6,TIM_IT_Update) == SET)
	{
		for(temp = 0;temp < 4;temp++)
		{
			Disitance_data[temp] = distance[temp];//赋值
			
		}
		
		Disitance_clear();//计数器清空
		TIM_ClearITPendingBit(TIM6,TIM_IT_Update);
	}
}

//定时器7 PID运算
void TIM7_IRQHandler(void)
{
	u8 temp;
	if(TIM_GetITStatus(TIM7,TIM_IT_Update) == SET)
	{
		for(temp = 0;temp<4;temp++)
		{
			Motor_PID_compute(&(Motor_PID_f_data[temp]),Motor_target_data[temp],Read_Speed(temp));
			Motor_PID_compute(&(Motor_PID_b_data[temp]),Motor_target_data[temp],Read_Speed(temp));
		}
		//USART5_PrintDatas("time7\n");
		TIM_ClearITPendingBit(TIM7,TIM_IT_Update);
	}
}

